﻿// SCurveTest.cpp : 此文件包含 "main" 函数。程序执行将在此处开始并结束。
//

#include <iostream>
#include "math.h"
#include "time.h"
#include "stdarg.h"
#include "stdio.h"
#include <fstream>

using namespace std;

#undef sq
#define sq(x) ((x)*(x))

#define S_CURVE_ACCELERATION 1
#define CEIL(x)     ceilf(x)
#define FLOOR(x)    floorf(x)
#define SQRT(x)     sqrtf(x)

// Use NUM_ARGS(__VA_ARGS__) to get the number of variadic arguments
#define _NUM_ARGS(_,Z,Y,X,W,V,U,T,S,R,Q,P,O,N,M,L,K,J,I,H,G,F,E,D,C,B,A,OUT,...) OUT
#define NUM_ARGS(...) _NUM_ARGS(0,__VA_ARGS__,26,25,24,23,22,21,20,19,18,17,16,15,14,13,12,11,10,9,8,7,6,5,4,3,2,1,0)

#define _MIN(a,b)      ((a)<(b)?(a):(b))
#define _MAX(a,b)      ((a)>(b)?(a):(b))

#define _ISENA_     ~,1
#define _ISENA_1    ~,1
#define _ISENA_0x1  ~,1
#define _ISENA_true ~,1
#define _ISENA(V, ...)        IS_PROBE(V)

#define _ENA_1(O)           _ISENA(CAT(_IS,CAT(ENA_, O)))
#define _DIS_1(O)           NOT(_ENA_1(O))
#define ENABLED(...)       DO(ENA,&&,V)
#define DISABLED(V, ...)      DO(DIS,&&,V)


#undef NOLESS
#define NOLESS(v, n) \
    do{ \
      if (n > v) v = n; \
    }while(0)

#undef NOMORE
#define NOMORE(v, n) \
    do{ \
      __typeof__(n) _n = (n); \
      if (_n < v) v = _n; \
    }while(0)

#undef LIMIT
#define LIMIT(v, n1, n2) \
    do{ \
      __typeof__(n1) _n1 = (n1); \
      __typeof__(n2) _n2 = (n2); \
      if (_n1 > v) v = _n1; \
      else if (_n2 < v) v = _n2; \
    }while(0)

#define MINIMAL_STEP_RATE 0.0

#undef DInterpolationCycle
#define DInterpolationCycle 0.001			// 1ms

/**
 * struct block_t
 *
 * A single entry in the planner buffer.
 * Tracks linear movement over multiple axes.
 *
 * The "nominal" values are as-specified by gcode, and
 * may never actually be reached due to acceleration limits.
 */
typedef struct block_t {

    volatile uint8_t flag;                    // Block flags (See BlockFlag enum above) - Modified by ISR and main thread!

    // Fields used by the motion planner to manage acceleration
	float nominal_speed_sqr;                  // The nominal speed for this block in (mm/sec)^2
	float entry_speed_sqr;	                  // Entry speed at previous-current junction in (mm/sec)^2
	float max_entry_speed_sqr;                // Maximum allowable junction entry speed in (mm/sec)^2
	float millimeters;                        // The total travel of this block in mm
    float acceleration;                       // acceleration mm/sec^2
/*
    union {
        abce_ulong_t steps;                     // Step count along each axis
        abce_long_t position;                   // New position to force when this sync block is executed
    };
*/

    float step_event_count;                // The number of step events required to complete this block

    // Settings for the trapezoid generator
	float accelerate_until;                // The index of the step event on which to stop acceleration
    float decelerate_after;                // The index of the step event on which to start decelerating

#ifdef S_CURVE_ACCELERATION
	float cruise_rate;                   // The actual cruise rate to use, between end of the acceleration phase and start of deceleration phase
	float acceleration_time;             // Acceleration time and deceleration time in STEP timer counts
	float deceleration_time;
	float acceleration_time_inverse;     // Inverse of acceleration and deceleration periods, expressed as integer. Scale depends on CPU being used
	float deceleration_time_inverse;
#else
    uint32_t acceleration_rate;             // The acceleration rate used for acceleration calculation
#endif

    uint8_t direction_bits;                   // The direction bit set for this block (refers to *_DIRECTION_BIT in config.h)
	float nominal_rate;                    // The nominal step rate for this block in step_events/sec
	float initial_rate;                    // The jerk-adjusted step rate at start of block
	float final_rate;                      // The minimal rate at exit
    float acceleration_steps_per_s2;       // acceleration steps/sec^2

	float bezier_A;
	float bezier_B;
	float bezier_C;
	float bezier_F;
	float bezier_AV;
} block_t;



float bezier_A;
float bezier_B;
float bezier_C;
float bezier_F;
float bezier_AV;			// 总距离的倒数


/**
    * Calculate the distance (not time) it takes to accelerate
    * from initial_rate to target_rate using the given acceleration:
    */
float estimate_acceleration_distance(const float& initial_rate, const float& target_rate, const float& accel) {
    if (accel == 0) return 0; // accel was 0, set acceleration distance to 0
    return (sq(target_rate) - sq(initial_rate)) / (accel * 2);
}

/**
 * Return the point at which you must start braking (at the rate of -'accel') if
 * you start at 'initial_rate', accelerate (until reaching the point), and want to end at
 * 'final_rate' after traveling 'distance'.
 *
 * This is used to compute the intersection point between acceleration and deceleration
 * in cases where the "trapezoid" has no plateau (i.e., never reaches maximum speed)
 */
float intersection_distance(const float& initial_rate, const float& final_rate, const float& accel, const float& distance) {
    if (accel == 0) return 0; // accel was 0, set intersection distance to 0
    return (accel * 2 * distance - sq(initial_rate) + sq(final_rate)) / (accel * 4);
}

static float final_speed(float initial_velocity, float accel, float distance) {
	return SQRT(sq(initial_velocity) + 2 * accel * distance);
}

//static FORCE_INLINE uint32_t get_period_inverse(const uint32_t d) {
static float get_period_inverse(const uint32_t d) {
	return d ? 0xFFFFFFFF / d : 0xFFFFFFFF;
}

/**
 * Calculate trapezoid parameters, multiplying the entry- and exit-speeds
 * by the provided factors.
 **
 * ############ VERY IMPORTANT ############
 * NOTE that the PRECONDITION to call this function is that the block is
 * NOT BUSY and it is marked as RECALCULATE. That WARRANTIES the Stepper ISR
 * is not and will not use the block while we modify it, so it is safe to
 * alter its values.
 */
void calculate_trapezoid_for_block(block_t* const block, const float& entry_factor, const float& exit_factor) 
{
	float initial_rate = block->initial_rate * entry_factor;
	float final_rate = block->final_rate * exit_factor;

// Limit minimal step rate (Otherwise the timer will overflow.)
    NOLESS(initial_rate, MINIMAL_STEP_RATE);
    NOLESS(final_rate, MINIMAL_STEP_RATE);

	#ifdef  S_CURVE_ACCELERATION
	float cruise_rate = initial_rate;
	#endif

	float accel = block->acceleration;// block->acceleration_steps_per_s2;

    // Steps required for acceleration, deceleration to/from nominal rate
	float accelerate_steps = estimate_acceleration_distance(initial_rate, block->nominal_rate, accel);
	float decelerate_steps = estimate_acceleration_distance(block->nominal_rate, final_rate, -accel);
    // Steps between acceleration and deceleration, if any
	float plateau_steps = block->step_event_count - accelerate_steps - decelerate_steps;

    // Does accelerate_steps + decelerate_steps exceed step_event_count?
    // Then we can't possibly reach the nominal rate, there will be no cruising.
    // Use intersection_distance() to calculate accel / braking time in order to
    // reach the final_rate exactly at the end of this block.
    if (plateau_steps < 0) {
        const float accelerate_steps_float = CEIL(intersection_distance(initial_rate, final_rate, accel, block->step_event_count));
        accelerate_steps = _MIN(uint32_t(_MAX(accelerate_steps_float, 0)), block->step_event_count);
        plateau_steps = 0;
		
		#ifdef S_CURVE_ACCELERATION
			// We won't reach the cruising rate. Let's calculate the speed we will reach
			cruise_rate = final_speed(initial_rate, accel, accelerate_steps);
		#endif
    }
	#ifdef S_CURVE_ACCELERATION
    else // We have some plateau time, so the cruise rate will be the nominal rate
        cruise_rate = block->nominal_rate;
	#endif

	#ifdef S_CURVE_ACCELERATION
		// Jerk controlled speed requires to express speed versus time, NOT steps
	float acceleration_time = (float)(cruise_rate - initial_rate) / accel;
	float deceleration_time = (float)(cruise_rate - final_rate) / accel;
			// And to offload calculations from the ISR, we also calculate the inverse of those times here
	float acceleration_time_inverse = get_period_inverse(acceleration_time);
	float deceleration_time_inverse = get_period_inverse(deceleration_time);
	#endif

    // Store new block parameters
    block->accelerate_until = accelerate_steps;
    block->decelerate_after = accelerate_steps + plateau_steps;
    block->initial_rate = initial_rate;
	#ifdef S_CURVE_ACCELERATION
		block->acceleration_time = acceleration_time;
		block->deceleration_time = deceleration_time;
		block->acceleration_time_inverse = acceleration_time_inverse;
		block->deceleration_time_inverse = deceleration_time_inverse;
		block->cruise_rate = cruise_rate;
	#endif
    block->final_rate = final_rate;
}

// For all the other 32bit CPUs
void _calc_bezier_curve_coeffs(float v0, float v1, float av)
{
	bezier_A = 6.0 * (v1 - v0);
	bezier_B = 15.0 * (v0 - v1);
	bezier_C = 10 * (v1 - v0);
	// bezier_D = 0;
	// bezier_E = 0;
	bezier_F = v0;
	bezier_AV = av;
#if 0
	// Calculate the Bézier coefficients
	bezier_A = 768 * (v1 - v0);
	bezier_B = 1920 * (v0 - v1);
	bezier_C = 1280 * (v1 - v0);
	bezier_F = 128 * v0;
	bezier_AV = av;
#endif

}

float _eval_bezier_curve(const float curr_step) {
#if defined(__arm__) || defined(__thumb__)

	// For ARM Cortex M3/M4 CPUs, we have the optimized assembler version, that takes 43 cycles to execute
	uint32_t flo = 0;
	uint32_t fhi = bezier_AV * curr_step;
	uint32_t t = fhi;
	int32_t alo = bezier_F;
	int32_t ahi = 0;
	int32_t A = bezier_A;
	int32_t B = bezier_B;
	int32_t C = bezier_C;

	__asm__ __volatile__(
		".syntax unified" "\n\t"              // is to prevent CM0,CM1 non-unified syntax
		A("lsrs  %[ahi],%[alo],#1")           // a  = F << 31      1 cycles
		A("lsls  %[alo],%[alo],#31")          //                   1 cycles
		A("umull %[flo],%[fhi],%[fhi],%[t]")  // f *= t            5 cycles [fhi:flo=64bits]
		A("umull %[flo],%[fhi],%[fhi],%[t]")  // f>>=32; f*=t      5 cycles [fhi:flo=64bits]
		A("lsrs  %[flo],%[fhi],#1")           //                   1 cycles [31bits]
		A("smlal %[alo],%[ahi],%[flo],%[C]")  // a+=(f>>33)*C;     5 cycles
		A("umull %[flo],%[fhi],%[fhi],%[t]")  // f>>=32; f*=t      5 cycles [fhi:flo=64bits]
		A("lsrs  %[flo],%[fhi],#1")           //                   1 cycles [31bits]
		A("smlal %[alo],%[ahi],%[flo],%[B]")  // a+=(f>>33)*B;     5 cycles
		A("umull %[flo],%[fhi],%[fhi],%[t]")  // f>>=32; f*=t      5 cycles [fhi:flo=64bits]
		A("lsrs  %[flo],%[fhi],#1")           // f>>=33;           1 cycles [31bits]
		A("smlal %[alo],%[ahi],%[flo],%[A]")  // a+=(f>>33)*A;     5 cycles
		A("lsrs  %[alo],%[ahi],#6")           // a>>=38            1 cycles
		: [alo] "+r"(alo),
		[flo]"+r"(flo),
		[fhi]"+r"(fhi),
		[ahi]"+r"(ahi),
		[A]"+r"(A),  // <== Note: Even if A, B, C, and t registers are INPUT ONLY
		[B]"+r"(B),  //  GCC does bad optimizations on the code if we list them as
		[C]"+r"(C),  //  such, breaking this function. So, to avoid that problem,
		[t]"+r"(t)    //  we list all registers as input-outputs.
		:
		: "cc"
	);
	return alo;

#else

	// For non ARM targets, we provide a fallback implementation. Really doubt it
	// will be useful, unless the processor is fast and 32bit


	// V_f(t) = A * t ^ 5 + B * t ^ 4 + C * t ^ 3 + F;
	float t = bezier_AV * curr_step;
	float t3 = t * t * t;
	float t4 = t3 * t;
	float t5 = t4 * t;

	return bezier_A * t5 + bezier_B * t4 + bezier_C * t3 + bezier_F;

#if 0
	uint32_t t = bezier_AV * curr_step;               // t: Range 0 - 1^32 = 32 bits
	uint64_t f = t;
	f *= t;                                           // Range 32*2 = 64 bits (unsigned)
	f >>= 32;                                         // Range 32 bits  (unsigned)
	f *= t;                                           // Range 32*2 = 64 bits  (unsigned)
	f >>= 32;                                         // Range 32 bits : f = t^3  (unsigned)
	int64_t acc = (int64_t)bezier_F << 31;           // Range 63 bits (signed)
	acc += ((uint32_t)f >> 1) * (int64_t)bezier_C;  // Range 29bits + 31 = 60bits (plus sign)
	f *= t;                                           // Range 32*2 = 64 bits
	f >>= 32;                                         // Range 32 bits : f = t^3  (unsigned)
	acc += ((uint32_t)f >> 1) * (int64_t)bezier_B;  // Range 29bits + 31 = 60bits (plus sign)
	f *= t;                                           // Range 32*2 = 64 bits
	f >>= 32;                                         // Range 32 bits : f = t^3  (unsigned)
	acc += ((uint32_t)f >> 1) * (int64_t)bezier_A;  // Range 28bits + 31 = 59bits (plus sign)
	acc >>= (31 + 7);                                 // Range 24bits (plus sign)
	return (int32_t)acc;
#endif

#endif
}




/*****************************************************************************
* FUNCTION:			LineInterpolation
*
* DESCRIPTION:
*  直线插补
* 返回值：
0-插补成功
-1-当前示教指令已经插补完成
* 输出值:
double DValue[]
\*****************************************************************************/
struct stAxisInfoNodeTyp{
	double fVStart;
	double fVEnd;
	double fVCruise;
	double fVCmnd;
	double fDist;
	double fAcc;
	double fDec;
	double fLenAcc;
	double fLenDec;
	double fLenCruise;
	int32_t nAccCnt;
	int32_t nDecCnt;
	int32_t nCruiseCnt;
	int32_t nTotalCnt;
};

/*****************************************************************************
* FUNCTION:			UpdateAcc
*
* DESCRIPTION: 用于加减速度重新规划，确保插补精度
输入参数 double T1,double T2, double T3(加速时间 匀速时间 减速时间)
		 double Vs,double Ve,double Len(起点速度 结束点速度 总长)
输出参数 加速度 减速度
\*****************************************************************************/
void UpdateAcc(double T1, double T2, double T3, double Vs, double Ve, double Len, double* ADcc)
{
	ADcc[0] = -(1.0 * (T3 * Ve - 2.0 * Len + 2.0 * T1 * Vs + 2.0 * T2 * Vs + T3 * Vs)) / (T1 * (T1 + 2.0 * T2 + T3));


	ADcc[1] = -(1.0 * (T1 * Ve - 2.0 * Len + 2.0 * T2 * Ve + 2.0 * T3 * Ve + T1 * Vs))
		/ (T1 * T3 + 2.0 * T2 * T3 + T3 * T3);
}

/*
int calculate_trapezoid_for_block(struct stAxisInfoNodeTyp *pNod, double fMaxAcc, double fMaxDec, double fFactor)
{
	double sx, sy, sz;         //起点坐标
	double ex, ey, ez;         //结束点坐标

	double Ta, Tc, Td;         //加速时间，匀速时间，减速时间
	double Sa, Sc, Sd;         //加速段距离，匀速段距离，减速段距离
	double svel, cvel, evel;   //起点速度，匀速运行速度，结束点速度

	int i;                     //for语句的循环变量
	double  ADcc[2];           //用于加减速更新

	double Tm;

	// 计算两点之间直线长度
	double TotalLength = fabs(pNod->fDist);

	svel = pNod->fVStart;
	cvel = pNod->fVCmnd;
	evel = pNod->fVEnd;

	// 20160322邹修改
	pNod->fAcc = cvel * 10;
	pNod->fDec = cvel * 10;

	// 最大加减速速度限制
	if (pNod->fAcc > fMaxAcc){
		pNod->fAcc = fMaxAcc;
	}

	if (pNod->fDec > fMaxDec){
		pNod->fAcc = fMaxDec;
	}

	// 计算加速时间
	Ta = fabs((cvel - svel) / pNod->fAcc);
	Td = fabs((cvel - evel) / pNod->fDec);

	// 计算加减速段的长度
	Sa = (cvel + svel) * Ta / 2;
	Sd = (cvel + evel) * Td / 2;

	//匀速段长度
	Sc = TotalLength - Sa - Sd;

	//当匀速段时间小于0，说明设定速度值太大应该降低设置速度值
	if (Sc < 0){
		cvel = sqrt((svel * svel + evel * evel
			+ 2 * pNod->fAcc * TotalLength) / 2);
		Ta = fabs((cvel - svel) / pNod->fAcc);
		Td = fabs((cvel - evel) / pNod->fDec);
		Tc = 0;
	}
	else {
		// 匀速段时间
		Tc = Sc / cvel;
	}

	//更新运行速度
	pNod->fVCruise = cvel;

	//计算加速段插补点数
	pNod->nAccCnt = (int)(Ta / DInterpolationCycle) + 1;

	//计算匀速段插补点数
	pNod->nCruiseCnt = (int)(Tc / DInterpolationCycle) + 1;

	//计算减速段插补点数
	pNod->nDecCnt = (int)(Td / DInterpolationCycle) + 1;

	//总插补点数
	pNod->nTotalCnt = pNod->nAccCnt + pNod->nDecCnt + pNod->nCruiseCnt;

	//由于插补点取整误差，需要重新规划加减速度
	UpdateAcc(pNod->nAccCnt * DInterpolationCycle, pNod->nCruiseCnt * DInterpolationCycle, pNod->nDecCnt * DInterpolationCycle,
		pNod->fVStart, pNod->fVEnd, pNod->fDist, ADcc);
	pNod->fAcc = ADcc[0];
	pNod->fDec = ADcc[1];

	// 计算最后可以达到的指令速度
	pNod->fVCruise = pNod->fVStart + pNod->fAcc * pNod->nAccCnt * DInterpolationCycle;

	// 计算加速、减速和匀速段的长度
	pNod->fLenAcc = (pNod->fVStart + pNod->fVCruise) * pNod->nAccCnt * DInterpolationCycle * 0.5;
	pNod->fLenDec = (pNod->fVEnd + pNod->fVCruise) * pNod->nDecCnt * DInterpolationCycle * 0.5;
	pNod->fLenCruise = pNod->fDist - pNod->fLenAcc - pNod->fLenDec;

	return 0;
}
*/

int main()
{
    std::cout << "Hello World!\n";
	ofstream outfile("out.txt", ios::trunc);

#if 0
	struct stAxisInfoNodeTyp stAxisInfoNod;
	memset(&stAxisInfoNod, 0x00, sizeof(stAxisInfoNod));

	std::cout << "Index\t\tVStart\t\tVEnd\t\tVCmnd\t\tVSet\t\tfAcc\t\tfDec\t\t";
	std::cout << "nAcc\t\tnDec\t\tnCruise\t\tLenAcc\t\tLenDec\t\tLenCruise\n" << std::endl;

	srand(time(NULL));
	for (int32_t nIndx = 0; nIndx < 100; nIndx ++) {
		stAxisInfoNod.fVStart = rand() % 100;
		//stAxisInfoNod.fVEnd   = rand() % 100;
		stAxisInfoNod.fVEnd = 0.0;
		stAxisInfoNod.fVCmnd  = rand() % 100;
		stAxisInfoNod.fDist   = rand() % 200 + 50;

		//stAxisInfoNod.fVStart = 78;
		//stAxisInfoNod.fVEnd = 47;
		//stAxisInfoNod.fVCmnd = 78;
		//stAxisInfoNod.fDist = 250;

		
		if (stAxisInfoNod.fVCmnd < stAxisInfoNod.fVStart) {
			stAxisInfoNod.fVCmnd = stAxisInfoNod.fVStart;
		}

		if (stAxisInfoNod.fVCmnd < stAxisInfoNod.fVEnd) {
			stAxisInfoNod.fVCmnd = stAxisInfoNod.fVEnd;
		}

		//calculate_trapezoid_for_block(&stAxisInfoNod, 500.0, 500.0, 1.0);

		char tmpOutBuf[256] = {0};
		sprintf_s(tmpOutBuf, "%03d:\t%10.3f\t%10.3f\t%10.3f\t%10.3f\t", nIndx, stAxisInfoNod.fVStart, stAxisInfoNod.fVEnd, stAxisInfoNod.fVCmnd, stAxisInfoNod.fVCruise);
		
		std::cout << tmpOutBuf;

		sprintf_s(tmpOutBuf, "%10.3f\t%10.3f\t%10d\t%10d\t%10d\t", stAxisInfoNod.fAcc, stAxisInfoNod.fDec, stAxisInfoNod.nAccCnt, stAxisInfoNod.nDecCnt, stAxisInfoNod.nCruiseCnt);

		std::cout << tmpOutBuf;

		sprintf_s(tmpOutBuf, "%10.3f\t%10.3f\t%10.3f\t%10.3f\t", stAxisInfoNod.fLenAcc, stAxisInfoNod.fLenDec, stAxisInfoNod.fLenCruise, stAxisInfoNod.fDist);

		std::cout << tmpOutBuf << "\n";

/*
		std::cout << nIndx << ":\t\t";
		std::cout << stAxisInfoNod.fVStart << "\t\t" << stAxisInfoNod.fVEnd << "\t\t" << stAxisInfoNod.fVCmnd << "\t\t" << stAxisInfoNod.fVCruise << "\t\t";
		std::cout << stAxisInfoNod.nAccCnt << "\t\t" << stAxisInfoNod.nDecCnt << "\t\t" << stAxisInfoNod.nCruiseCnt << "\n";
*/

	}	
#else
	block_t stAxisInfoNod;
	memset(&stAxisInfoNod, 0x00, sizeof(stAxisInfoNod));

	stAxisInfoNod.acceleration = 200.0;
	char tmpOutBuf[256] = { 0 };
	sprintf_s(tmpOutBuf, "Index\t\tVStart\t\tVEnd\t\tVCmnd\t\tVSet\t\tDist\t\tLenAcc\t\tLenDec\n");
	std::cout << tmpOutBuf;
//	outfile << tmpOutBuf;

	
	//srand(time(NULL));
	for (int32_t nIndx = 0; nIndx < 1; nIndx++) {
		//stAxisInfoNod.initial_rate = rand() % 100;
		//stAxisInfoNod.fVEnd   = rand() % 100;
		stAxisInfoNod.initial_rate = 20;
		stAxisInfoNod.final_rate = 0.0;
		stAxisInfoNod.nominal_rate = rand() % 100;
		stAxisInfoNod.step_event_count = rand() % 200 + 50;

		//stAxisInfoNod.fVStart = 78;
		//stAxisInfoNod.fVEnd = 47;
		//stAxisInfoNod.fVCmnd = 78;
		//stAxisInfoNod.fDist = 250;


		if (stAxisInfoNod.nominal_rate < stAxisInfoNod.initial_rate) {
			stAxisInfoNod.nominal_rate = stAxisInfoNod.initial_rate;
		}

		if (stAxisInfoNod.nominal_rate < stAxisInfoNod.final_rate) {
			stAxisInfoNod.nominal_rate = stAxisInfoNod.final_rate;
		}

		calculate_trapezoid_for_block(&stAxisInfoNod, 1.0, 1.0);

		sprintf_s(tmpOutBuf, "%03d\t%10.3f\t%10.3f\t%10.3f\t%10.3f\t", nIndx, stAxisInfoNod.initial_rate, stAxisInfoNod.final_rate, stAxisInfoNod.nominal_rate, stAxisInfoNod.cruise_rate);

		std::cout << tmpOutBuf;
		//outfile << tmpOutBuf;

		sprintf_s(tmpOutBuf, "%10.3f\t%10.3f\t%10.3f\t", stAxisInfoNod.step_event_count, stAxisInfoNod.accelerate_until, stAxisInfoNod.decelerate_after);

		std::cout << tmpOutBuf << "\n";
		//outfile << tmpOutBuf;

		float time = 0.0;
		float tmpVel = stAxisInfoNod.initial_rate;
		float tmpSumDist = 0.0;

		#ifdef eval_dist
			_calc_bezier_curve_coeffs(stAxisInfoNod.initial_rate, stAxisInfoNod.cruise_rate, 1.0 / stAxisInfoNod.accelerate_until);

			while (1)
			{
				// 加速段
				sprintf_s(tmpOutBuf, "%10.3f\t%10.3f\t%10.3f\n", time, tmpVel, tmpSumDist);
				std::cout << tmpOutBuf;
				outfile << tmpOutBuf;

				if (tmpSumDist < stAxisInfoNod.accelerate_until) {
					float tmpT = tmpSumDist;
					tmpVel = _eval_bezier_curve(tmpT);
				}
				else if (tmpSumDist > stAxisInfoNod.step_event_count) {
					tmpSumDist = stAxisInfoNod.step_event_count;
					tmpVel = stAxisInfoNod.final_rate;
					break;
				}
				else if(tmpSumDist > stAxisInfoNod.decelerate_after){
					if (0 == stAxisInfoNod.flag) {
						stAxisInfoNod.flag = 1;
						_calc_bezier_curve_coeffs(stAxisInfoNod.cruise_rate, stAxisInfoNod.final_rate, 1.0 / (stAxisInfoNod.step_event_count - stAxisInfoNod.decelerate_after));
					}
					float tmpT = tmpSumDist - stAxisInfoNod.decelerate_after;
					tmpVel = _eval_bezier_curve(tmpT);
				}
				else {
					tmpVel = stAxisInfoNod.cruise_rate;
				}

				tmpSumDist += tmpVel * DInterpolationCycle;
				time += DInterpolationCycle;
			}
		#else
		float timAcc = 0.0;
		float timCurise = 0.0;
		float timDec = 0.0;
			_calc_bezier_curve_coeffs(stAxisInfoNod.initial_rate, stAxisInfoNod.cruise_rate, 1.0 / stAxisInfoNod.acceleration_time);
			while (1)
			{
				// 加速段
				sprintf_s(tmpOutBuf, "%10.3f\t%10.3f\t%10.3f\t%10.3f\n", time, tmpVel, tmpSumDist, stAxisInfoNod.step_event_count - tmpSumDist);
				std::cout << tmpOutBuf;
				outfile << tmpOutBuf;

				if (tmpSumDist > stAxisInfoNod.step_event_count) {
					tmpSumDist = stAxisInfoNod.step_event_count;
					tmpVel = stAxisInfoNod.final_rate;
					break;
				}
				else {
					if (tmpSumDist <= stAxisInfoNod.accelerate_until) {
						timAcc += DInterpolationCycle;
						tmpVel = _eval_bezier_curve(timAcc);
						if (tmpVel > stAxisInfoNod.cruise_rate) {
							tmpVel = stAxisInfoNod.cruise_rate;
						}
					}
					else if (tmpSumDist > stAxisInfoNod.decelerate_after) {
						timDec += DInterpolationCycle;
						if (0 == stAxisInfoNod.flag) {
							stAxisInfoNod.flag = 1;
							_calc_bezier_curve_coeffs(stAxisInfoNod.cruise_rate, stAxisInfoNod.final_rate, 1.0 / stAxisInfoNod.deceleration_time);
						}

						tmpVel = _eval_bezier_curve(timDec);
						if ((stAxisInfoNod.final_rate < 0.1) && (tmpVel < 0.1)) {
							tmpVel = 0.1;
						}
					}
					else {
						timCurise += DInterpolationCycle;
						tmpVel = stAxisInfoNod.cruise_rate;
					}

					tmpSumDist += tmpVel * DInterpolationCycle;
					time += DInterpolationCycle;
				}
			}
		#endif

		/*
				std::cout << nIndx << ":\t\t";
				std::cout << stAxisInfoNod.fVStart << "\t\t" << stAxisInfoNod.fVEnd << "\t\t" << stAxisInfoNod.fVCmnd << "\t\t" << stAxisInfoNod.fVCruise << "\t\t";
				std::cout << stAxisInfoNod.nAccCnt << "\t\t" << stAxisInfoNod.nDecCnt << "\t\t" << stAxisInfoNod.nCruiseCnt << "\n";
		*/
	}
	outfile.close();

#endif
}

// 运行程序: Ctrl + F5 或调试 >“开始执行(不调试)”菜单
// 调试程序: F5 或调试 >“开始调试”菜单

// 入门使用技巧: 
//   1. 使用解决方案资源管理器窗口添加/管理文件
//   2. 使用团队资源管理器窗口连接到源代码管理
//   3. 使用输出窗口查看生成输出和其他消息
//   4. 使用错误列表窗口查看错误
//   5. 转到“项目”>“添加新项”以创建新的代码文件，或转到“项目”>“添加现有项”以将现有代码文件添加到项目
//   6. 将来，若要再次打开此项目，请转到“文件”>“打开”>“项目”并选择 .sln 文件
